
#include "chassis.h"
#include "robot_def.h"

#include "general_def.h"
#include "bsp_dwt.h"
#include "arm_math.h"
#include "bsp_pwm.h"
#include "PWM_motor.h"
#include "tim.h"
#include "mpu6500.h"
#include "message_center.h"
#include "dji_motor.h"
#include "servo_motor.h"
static PWM_MOTOR_Instance_s *PWM_Motor_R, *PWM_Motor_L;
static DJIMotorInstance *m2006;

static attitude_t *gimba_IMU_data; // 云台IMU数据

static Publisher_t *chassis_pub;  // 用于发布底盘的数据
static Subscriber_t *chassis_sub; // 用于订阅底盘的控制命令

static Chassis_Ctrl_Cmd_s chassis_cmd_recv; // 底盘接收到的控制命令

#define ChassisWidth 12.0f
#define ChassisRadius (ChassisWidth / 2f)
#define WheelRadius 3.25f
// void ChassisInit()
// {
//     HAL_GPIO_WritePin(GPIOG, D8_Pin | D7_Pin | D6_Pin | D5_Pin | D4_Pin | D3_Pin | D2_Pin | D1_Pin, GPIO_PIN_SET);
//     gimba_IMU_data = INS_Init();
//     PWM_MOTOR_Init_Config_s PWM_Motor_L_Config = {
//         .pwm_init_config = {
//             .channel = TIM_CHANNEL_1,
//             .dutyratio = 0.5,
//             .htim = &htim2,
//             .period = 1,
//         },
//         .dir_gpio_1 = {
//             .gpio_port = GPIOC,
//             .gpio_pin = GPIO_PIN_2,
//         },
//         .dir_gpio_2 = {
//             .gpio_port = GPIOC,
//             .gpio_pin = GPIO_PIN_3,
//         },
//         .close_loop_type = ANGLE_LOOP,
//         .controller_param_init_config = {
//             .angle_PID = {
//                 .Kp = 0.75,
//                 .Ki = 0,
//                 .Kd = 0.015,
//                 .MaxOut = 100,
//             },
//             .speed_PID = {
//                 .Kp = 10,
//                 .Ki = 0,
//                 .Kd = 0,
//                 .MaxOut = 100,
//             },
//             .other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle,
//         },
//         .angle_feedback_source = MOTOR_FEED,
//         .control_type = PWM_MOTOR_CLOSELOOP,
//         .direction = PWM_MOTOR_DIRECTION_NORMAL,
//         .encoder_tim = &htim8,
//         .angle_feedback_dir = FEEDBACK_DIRECTION_NORMAL,
//         .speed_feedback_dir = FEEDBACK_DIRECTION_NORMAL,
//     };
//     PWM_Motor_L = PWMMotorInit(&PWM_Motor_L_Config);
//     PWM_MOTOR_Init_Config_s PWM_Motor_R_Config = {
//         .pwm_init_config = {
//             .channel = TIM_CHANNEL_1,
//             .dutyratio = 0.5,
//             .htim = &htim4,
//             .period = 1,
//         },
//         .dir_gpio_1 = {
//             .gpio_port = GPIOE,
//             .gpio_pin = GPIO_PIN_5,
//         },
//         .dir_gpio_2 = {
//             .gpio_port = GPIOE,
//             .gpio_pin = GPIO_PIN_6,
//         },
//         .close_loop_type = ANGLE_LOOP,
//         .controller_param_init_config = {
//             .angle_PID = {
//                 .Kp = 0.75,
//                 .Ki = 0,
//                 .Kd = 0.015,
//                 .MaxOut = 100,
//             },
//             .speed_PID = {
//                 .Kp = 10,
//                 .Ki = 0,
//                 .Kd = 0,
//                 .MaxOut = 100,
//             },
//             .other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle,
//         },
//         .control_type = PWM_MOTOR_CLOSELOOP,
//         .direction = PWM_MOTOR_DIRECTION_REVERSE,
//         .encoder_tim = &htim5,
//         .angle_feedback_source = MOTOR_FEED,
//         .angle_feedback_dir = FEEDBACK_DIRECTION_NORMAL,
//         .speed_feedback_dir = FEEDBACK_DIRECTION_NORMAL,
//     };
//     PWM_Motor_R = PWMMotorInit(&PWM_Motor_R_Config);
//     Motor_Init_Config_s m2006_config = {
//         .can_init_config  = {
//             .can_handle = &hcan1,
//             .tx_id = 1
//         },
//         .motor_type = M2006,
//         .controller_param_init_config = {
//             .angle_PID = {
//                 .Kp = 0,
//                 .Ki = 0,
//                 .Kd = 0,
//                 .MaxOut = 100,
//             },
//             .speed_PID ={
//                 .Kp = 1,
//                 .Ki = 0,
//                 .Kd = 0,
//                 .MaxOut = 2000,
//             },
//             },
//             .controller_setting_init_config={
//                 .close_loop_type = SPEED_LOOP,
//                 .outer_loop_type = SPEED_LOOP,
//                 .speed_feedback_source = MOTOR_FEED,
//                 .angle_feedback_source = MOTOR_FEED,
//             }
//         };

//     m2006 = DJIMotorInit(&m2006_config);
//     chassis_sub = SubRegister("chassis_cmd", sizeof(Chassis_Ctrl_Cmd_s));
// }

// 陀螺仪顺时针为负
// void ChassisTask()
// {
//     DJIMotorSetRef(m2006, 2000);
//     SubGetMessage(chassis_sub, &chassis_cmd_recv);

//     switch (chassis_cmd_recv.chassis_mode)
//     {
//     case CHASSIS_STOP:
//         PWMMotorSetWorkingState(PWM_Motor_R, PWM_MOTOR_OFF);
//         PWMMotorSetWorkingState(PWM_Motor_L, PWM_MOTOR_OFF);

//         break;
//     case CHASSIS_Straight:
//         PWMMotorSetWorkingState(PWM_Motor_R, PWM_MOTOR_ON);
//         PWMMotorSetWorkingState(PWM_Motor_L, PWM_MOTOR_ON);

//         PWMMotorSetControLoopType(PWM_Motor_R, PWM_MOTOR_OPENLOOP);
//         PWMMotorSetControLoopType(PWM_Motor_L, PWM_MOTOR_OPENLOOP);

//         PWMMotorSetDirection(PWM_Motor_R, PWM_MOTOR_DIRECTION_REVERSE);
//         PWMMotorSetDirection(PWM_Motor_L, PWM_MOTOR_DIRECTION_NORMAL);

//         PWMMotorSetSpeed(PWM_Motor_R, chassis_cmd_recv.speed);
//         PWMMotorSetSpeed(PWM_Motor_L, chassis_cmd_recv.speed);

//         break;
//     case CHASSIS_Transverse:
//         PWMMotorSetWorkingState(PWM_Motor_R, PWM_MOTOR_ON);
//         PWMMotorSetWorkingState(PWM_Motor_L, PWM_MOTOR_ON);

//         PWMMotorSetControLoopType(PWM_Motor_R, PWM_MOTOR_CLOSELOOP);
//         PWMMotorSetControLoopType(PWM_Motor_L, PWM_MOTOR_CLOSELOOP);

//         PWMMotorSetClossingLoopType(PWM_Motor_R, ANGLE_AND_SPEED_LOOP);
//         PWMMotorSetClossingLoopType(PWM_Motor_L, ANGLE_AND_SPEED_LOOP);

//         PWMMotorSetRef(PWM_Motor_R, chassis_cmd_recv.speed);
//         PWMMotorSetRef(PWM_Motor_L, -chassis_cmd_recv.speed);
//         break;
//     default:
//         PWMMotorSetWorkingState(PWM_Motor_R, PWM_MOTOR_OFF);
//         PWMMotorSetWorkingState(PWM_Motor_L, PWM_MOTOR_OFF);

//         break;
//     }
// }